#include "physics.h"

void Euler() {

    if( TIME > FINISH_TIME )
        return;

    computeAcceleration( myParticles , myBox );
    resolveCollision( myParticles , myBox );

    Vector displacement;
    int i;
    for(i=0; i<myParticles.number; i++) {
        SCALE( myParticles.velocity[i] , STEPSIZE , displacement )
        ADD( myParticles.position[i] , displacement , myParticles.position[i] )
        SCALE( myParticles.acceleration[i] , STEPSIZE , myParticles.acceleration[i] )
        ADD( myParticles.velocity[i] , myParticles.acceleration[i] , myParticles.velocity[i] )
    }

    TIME += 1;

}

void RK4() {

}

void computeAcceleration( Particles &myParticles , Box &myBox ) {
    // compute force of each object
    int i;
    Vector a;
    for(i=0; i<myParticles.number; i++) {
        SCALE( myParticles.velocity[i] , DRAGCOEF , a )      // drag force
        ADD( a , WIND , a )                                  // wind force
        SCALE( a , myParticles.massInv[i] , a )              // to acceleration
        ADD( a , GRAVITATION , myParticles.acceleration[i] ) // add gravation
    }
}

void resolveCollision( Particles &myParticles , Box &myBox ) {

    int i;
    Vector temp;
    for(i=0; i< myParticles.number; i++) {
        if( myParticles.position[i].x > myBox.size ) {
            SETVECTOR( temp , 1 , 0 , 0 )
            SCALE( temp , -1*( myParticles.position[i].x - myBox.size ) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )
            SETVECTOR( temp , 1 , 0 , 0 )
            SCALE( temp , -0.8*(myParticles.velocity[i].x) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )
        }
        else if( myParticles.position[i].x < -myBox.size ) {
            SETVECTOR( temp , 1 , 0 , 0 )
            SCALE( temp , -1*( myParticles.position[i].x - myBox.size ) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )
            SETVECTOR( temp , 1 , 0 , 0 )
            SCALE( temp , -0.8*(myParticles.velocity[i].x) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )
        }
        if( myParticles.position[i].y > myBox.size ) {
            SETVECTOR( temp , 0 , 1 , 0 )
            SCALE( temp , -1*( myParticles.position[i].y - myBox.size ) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )
            SETVECTOR( temp , 0 , 1 , 0 )
            SCALE( temp , -0.8*(myParticles.velocity[i].y) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )
        }
        else if( myParticles.position[i].y < -myBox.size ) {
            SETVECTOR( temp , 0 , 1 , 0 )
            SCALE( temp , -1*( myParticles.position[i].y - myBox.size ) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )
            SETVECTOR( temp , 0 , 1 , 0 )
            SCALE( temp , -0.8*(myParticles.velocity[i].y) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )

        }
        if( myParticles.position[i].z > myBox.size ) {
            SETVECTOR( temp , 0 , 0 , 1 )
            SCALE( temp , -1*( myParticles.position[i].z - myBox.size ) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )
            SETVECTOR( temp , 0 , 0 , 1 )
            SCALE( temp , -0.8*(myParticles.velocity[i].z) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )

        }
        else if( myParticles.position[i].z < -myBox.size ) {
            SETVECTOR( temp , 0 , 0 , 1 )
            SCALE( temp , -1*( myParticles.position[i].z - myBox.size ) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )
            SETVECTOR( temp , 0 , 0 , 1 )
            SCALE( temp , -0.8*(myParticles.velocity[i].z) , temp )
            ADD( myParticles.acceleration[i] , temp , myParticles.acceleration[i] )

        }

    }

}
